#pragma config(Sensor, dgtl1,  ArmEncoderLeft,      sensorRotation)
#pragma config(Sensor, dgtl2,  ArmEncoderRight,     sensorRotation)
#pragma config(Sensor, dgtl3,  BaseEncoderLeft,     sensorRotation)
#pragma config(Sensor, dgtl4,  BaseEncoderRight,    sensorRotation)
#pragma config(Sensor, dgtl5,  LimitArmOne,         sensorTouch)
#pragma config(Sensor, dgtl6,  LimitArmTwo,         sensorTouch)
#pragma config(Sensor, dgtl7,  ProgramSwitchLeft,   sensorTouch)
#pragma config(Sensor, dgtl8,  ProgramSwitchRight,  sensorTouch)
#pragma config(Motor,  port2,           LeftBase,      tmotorNormal, openLoop, reversed)
#pragma config(Motor,  port4,           LeftArm,       tmotorNormal, openLoop, reversed)
#pragma config(Motor,  port5,           RightArm,      tmotorNormal, openLoop)
#pragma config(Motor,  port6,           LeftVacume,    tmotorNormal, openLoop, reversed)
#pragma config(Motor,  port7,           RightVacume,   tmotorNormal, openLoop)
#pragma config(Motor,  port8,           LeftTurntable, tmotorNormal, openLoop, reversed)
#pragma config(Motor,  port9,           RightTurntable, tmotorNormal, openLoop, reversed)
#pragma config(Motor,  port10,          RightBase,     tmotorNormal, openLoop)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

#pragma platform(VEX)

//Competition Control and Duration Settings
#pragma competitionControl(Competition)
#pragma autonomousDuration(20)
#pragma userControlDuration(120)

#include "Vex_Competition_Includes.c"   //Main competition background code...do not modify!


// Start Autonomous Voids
const int SquareAndAHalf = 40;
const int Square = 33;
bool VirtualButton = false;
void ResetEncoders()
{
  SensorValue(BaseEncoderLeft) = 0;
  SensorValue(BaseEncoderRight) = 0;
}

void ResetArmEncoders()
{
  SensorValue(ArmEncoderLeft) = 0;
  SensorValue(ArmEncoderRight) = 0;
}

int DistanceToCounts(int distance)//Distance in 1/10 inches!
{
  const int circumference = 12.6;//Large omni this variable can only go to tenths decimal
  int counts = 0;//1 count = 4 degrees, initially set to zero

  counts = (distance * 90) / (circumference);//Set counts for proper distance
  return counts;
}

void DriveMotorStop()
{
  motor[LeftBase] = 0;
  motor[RightBase] = 0;
}

void VacumePickUp()
{
  motor[LeftVacume] = 127;
  motor[RightVacume] = 127;
  wait1Msec(2000);
  motor[LeftVacume] = 0;
  motor[RightVacume] = 0;
}

void VacumeSpitOut()
{
    motor[LeftVacume] = 127;
    motor[RightVacume] = 127;
    wait1Msec(10000);
    motor[LeftVacume] = 0;
    motor[RightVacume] = 0;
}

void MoveStraight(int distance)
{

   int counts;//This is a variable to hold distance in counts

  ResetEncoders();//Reset encoders with function, start at zero!

  counts = DistanceToCounts(distance);

  while (SensorValue(BaseEncoderRight)<counts)
  {
    if (SensorValue(BaseEncoderLeft)>SensorValue(BaseEncoderRight))//Left faster
    {
      motor[LeftBase]=93/2;
      motor[RightBase]=103/2;
    }
    if (SensorValue(BaseEncoderLeft)<SensorValue(BaseEncoderRight))//Right faster
    {
      motor[LeftBase]=103/2;
      motor[RightBase]=93/2;
    }
    if (SensorValue(BaseEncoderLeft)==SensorValue(BaseEncoderRight))//Equal
    {
      motor[LeftBase]=103/2;
      motor[RightBase]=103/2;
    }
  }
  DriveMotorStop();
}


void MoveLift(int wantedposition)
{
  ResetArmEncoders();
  if(SensorValue(ArmEncoderLeft)<wantedposition)
  {
    while((SensorValue(ArmEncoderLeft)<wantedposition))
    {
      motor[LeftArm]=127;
      motor[RightArm]=127;
    }
    motor[LeftArm]=0;
    motor[RightArm]=0;
    return;
  }

  if(SensorValue(ArmEncoderLeft)>wantedposition && SensorValue(LimitArmOne) == 0)
  {
    while((SensorValue(ArmEncoderLeft)>wantedposition) && SensorValue(LimitArmOne) == 0)
    {
      motor[LeftArm]=-127;
      motor[RightArm]=-127;
    }
    motor[LeftArm]=0;
    motor[RightArm]=0;
    return;
  }
  return;
}

void MoveLiftDown()
{
  VirtualButton = true;
  while(VirtualButton == true)
  {
    if(SensorValue[LimitArmOne] == 0)
    {
      motor[LeftArm] = -127;
    }
    if(SensorValue[LimitArmTwo] == 0)
    {
      motor[RightArm] = -127;
    }
    if(SensorValue[LimitArmOne] == 1)
    {
      motor[LeftArm] = 0;
    }
    if(SensorValue[LimitArmTwo] == 1)
    {
      motor[RightArm] = 0;
    }
    if(SensorValue[LimitArmOne] == 1 && SensorValue[LimitArmTwo] == 1)
    {
      VirtualButton = false;
    }
  }
}

void TurnRobot (int turnangle)
{
  int wheelseperation = 12;   //Width of robot base
  int rotationdistance;       //Distance wheel needs to turn in 1/10 inches
  int wheelcounts;            //Counts wheel needs to travel
  int absturnangle;           //Absalute value of turnangle
  bool negative = false;      //If a number is negative, this variable gets notated "true."

  absturnangle = 0;

  if(turnangle<0)//If turnangle variable is negative...
  {
    negative = true;//...Then notate it throught variables.
  }

  absturnangle = abs(turnangle);//Compute absolute value (-90 becomes 90 if negative or 90 stays 90)

  rotationdistance = (wheelseperation * absturnangle * 3.14159265) / 36;  //Compute distance in 1/10"; PI = 3.14159265 here
  wheelcounts = DistanceToCounts(rotationdistance);  //Turn distance into counts
  bMotorFlippedMode[port3] = true;  //No reflection

  ResetEncoders();  //Reset encoders for use.

  if(negative == false)
  {
    while(SensorValue(BaseEncoderLeft)<wheelcounts)  //Runs when needed.
    {
      motor[LeftBase] = -63;
      motor[RightBase] = 63;
    }
  }
  else
  {
    while(SensorValue(BaseEncoderLeft)<wheelcounts)
    {
      motor[LeftBase]= 63;
      motor[RightBase]= -63;
    }
  }
  DriveMotorStop();
}

//**Port Mapping**//
//All port 2's are left side
//All port 3's are right side

// End Autonomous Voids

// Start UserControl Voids

void TurntableForward()
{
  if(vexRT[Btn6UXmtr2] == 1)
  {
    motor[LeftTurntable] = 127;
    motor[RightTurntable] = 127;
  }
  else
  {
    motor[LeftTurntable] = 5;
    motor[RightTurntable] = 5;
  }
}

void turnrobot90()
{
  motor[LeftBase] = 127;
  motor[RightBase] = -127;
  wait1msec(505);
  DriveMotorStop();
}

void turnrobotother90()
{
  motor[LeftBase] = -127;
  motor[RightBase] = 127;
  wait1msec(700);
  DriveMotorStop();
}


void TurntableBackward()
{
  if(vexRT[Btn6DXmtr2] == 1)
  {
    motor[LeftTurntable] = -127;
    motor[RightTurntable] = -127;
  }
}

void Base()
{
  motor(LeftBase) = vexRT(Ch2)/1.3;
  motor(RightBase) = vexRT(Ch3)/1.3;
}

void ArmsUp()
{
  if(vexRT[Btn7UXmtr2] == 1) // makes the arms go up if Btn 7 Up is pressed
  {
    motor[LeftArm] = 127;
    motor[RightArm] = 127;
  }
  else
  {
    motor[LeftArm] = 0;
    motor[RightArm] = 0;
  }
}

void ArmsDown()
{
  if(vexRT[Btn7DXmtr2] == 1 && SensorValue(LimitArmOne) == 0)  // makes the arms go down if Btn 7 Down is pressed
  {
    motor[LeftArm] = -127;
  }
  if(vexRT[Btn7DXmtr2] == 1 && SensorValue(LimitArmTwo) == 0)
  {
    motor[RightArm] = -127;
  }
}
void VacumeUp()
{
  if(vexRT[Btn8UXmtr2] == 1) // makes the vacume go up if Btn 8 up is pressed
  {
    motor[LeftVacume] = 127;
    motor[RightVacume] = 127;
  }
  else
  {
    motor[LeftVacume] = 0;
    motor[RightVacume] = 0;
  }
}

void VacumeDown()
{
  if(vexRT[Btn8DXmtr2] == 1) // makes the vacume go down if Btn 8 down is pressed
  {
    motor[LeftVacume] = -127;
    motor[RightVacume] = -127;
  }
}

// end UserControl Voids


/////////////////////////////////////////////////////////////////////////////////////////
//
//                          Pre-Autonomous Functions
//
// You may want to perform some actions before the competition starts. Do them in the
// following function.
//
/////////////////////////////////////////////////////////////////////////////////////////

void pre_auton()
{
	// All activities that occur before the competition starts
	// Example: clearing encoders, setting servo positions, ...
}

/////////////////////////////////////////////////////////////////////////////////////////
//
//                                 Autonomous Task
//
// This task is used to control your robot during the autonomous phase of a VEX Competition.
// You must modify the code to add your own robot specific commands here.
//
/////////////////////////////////////////////////////////////////////////////////////////

/* Key for Autonomous Switches
   Left and Right Switches not pressed, red iso zone
   Left Switch pressed and Right switch not, Blue iso zone
   Right Switch pressed and Left switch not, Red open zone
   Left Switch pressed and Right Switch pressed, Blue open zone*/

task autonomous()
{
  if(SensorValue(ProgramSwitchLeft) == 0 && SensorValue(ProgramSwitchRight) == 0) // autonomous mode for red iso zone
  {
    MoveStraight(SquareAndAHalf); // type in value for 1/10 of inch 240 = 1 square or 24 in. 1 square = 2 ft.
    TurnRobot90();
    MoveStraight(Square);
    TurnRobotother90();
    MoveStraight(SquareAndAHalf);
    VacumePickUp();
    VacumeSpitOut();
  }
  if(SensorValue(ProgramSwitchLeft) == 1 && SensorValue(ProgramSwitchRight) == 0) // autonomous mode for blue iso zone
  { // you can only preload one
    MoveStraight(SquareAndAHalf);
    TurnRobot(-90);
    MoveStraight(Square);
    TurnRobot(90);
    VacumePickUp();
    VacumeSpitOut();
  }
  if(SensorValue(ProgramSwitchLeft) == 0 && SensorValue(ProgramSwitchRight) == 1) // autonomous mode for red open
  { // you can only preload one
    MoveStraight(SquareAndAHalf);
    TurnRobot(90);
    MoveStraight(SquareAndAHalf);
  }
  if(SensorValue(ProgramSwitchLeft) == 1 && SensorValue(ProgramSwitchRight) == 1) // autonomous mode for blue open
  { // you can only preload one
    MoveStraight(SquareAndAHalf);
    TurnRobot(-90);
    MoveStraight(SquareAndAHalf);
  }
}

/////////////////////////////////////////////////////////////////////////////////////////
//
//                                 User Control Task
//
// This task is used to control your robot during the user control phase of a VEX Competition.
// You must modify the code to add your own robot specific commands here.
//
/////////////////////////////////////////////////////////////////////////////////////////

task usercontrol()
{
  while(1) // the code goes forever
  {
    TurntableForward(); // second remote
    TurntableBackward(); // second remote
    Base();
    ArmsUp(); // second remote
    ArmsDown(); // second remote
    VacumeUp(); // second remote
    VacumeDown(); // second remote
  }
}
